# -*- coding:utf-8 -*-
# @Time : 3/10/21 4:43 PM
# @Author :  rebeater
# @File : NavParser.py
# @Project:  NavigationTool_qt
# @Function: Nav文件读写转换

import numpy as np
import pandas as pd
import simplekml
from prettytable import PrettyTable

shapes_url = {
    'placemark_circle': 'http://maps.google.com/mapfiles/kml/shapes/placemark_circle.png',
    "shaped_dot": "http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png",
    "road_shield3": "http://maps.google.com/mapfiles/kml/shapes/road_shield3.png"
}
shape_size = 0.8
# 参考GNGGA语句中的q的含义定义颜色
color_list = {
    0: 'FFFF55FF',  # default
    1: 'FFFFFF55',
    2: 'FF00FFFF',
    3: 'FFFF0055',
    4: 'FF00FF00',
    5: 'FFFF0000',
    6: 'FFFF0FA0',
}


class NavParser:
    def __init__(self):
        self.data = None

    def to_csv(self, csv_path: str, fmt: str, header: str):
        pass

    def to_nav(self, nav_path: str, header: bool = False):
        with open(nav_path, 'w') as f:
            for d in self.data:
                f.write("%5d %.4f %.12f %.12f %.4f %.5f %.5f %.5f %.5f %.5f %.5f\n" %
                        (d[0], d[1], d[2], d[3], d[4], d[5], d[6], d[7], d[8], d[9], d[10])
                        )

    def split_by_time(self, start_time, end_time):
        self.data = self.data[self.data[:, 1] > start_time, :]
        self.data = self.data[self.data[:, 1] < end_time, :]

    def to_kml(self, kml_path: str,
               enable_different_color=True,
               point_color: str = "FFFF0000",
               track_color: str = "FFFF0000",
               down_sample=1,
               shape='shaped_dot'
               ):
        colors = {'blue': 'FFFF0000', 'red': "FF0000FF", 'green': 'FF00FF00', 'yellow': 'FF00FFFF', 'pink': 'FFFFFF00',
                  'orange': "FF00A5FF"
                  }
        if track_color in colors:
            trk_color_str = colors[track_color]
        else:
            trk_color_str = track_color
        if point_color in colors:
            pnt_color_str = colors[point_color]
        else:
            pnt_color_str = point_color

        # if enable_different_color:
        kml = simplekml.Kml()
        pnints = kml.newfolder(name="Points")
        pnt_styles = []
        for k in range(7):
            pnt_style = simplekml.Style()
            pnt_style.iconstyle.color = color_list[k] if enable_different_color else pnt_color_str
            pnt_style.iconstyle.scale = shape_size
            pnt_style.iconstyle.icon.href = shapes_url[
                shape]  # 'http://maps.google.com/mapfiles/kml/shapes/placemark_circle.png'
            pnt_styles.append(pnt_style)
        line_coords = []
        cnt = 0
        for d in self.data:
            cnt += 1
            if cnt % down_sample == 0:
                if np.fabs(d[3]) + np.fabs(d[2]) < 1e-4:# 避免（0，0）出現在地圖上
                    continue
                line_coords.append((d[3], d[2]))
                pnt = pnints.newpoint(
                    coords=[(d[3], d[2], d[4])])
                mode = int(d[11]) if int(d[11]) in color_list else 0
                pnt.style = pnt_styles[mode]
                pnt.description = self._description_str(d)
        lines = kml.newlinestring(name="track", description="", coords=line_coords)
        lines.style.linestyle.width = 1
        lines.style.linestyle.color = trk_color_str
        kml.save(kml_path)

    def _description_str(self, d):
        t = PrettyTable(["GPST", "Lat", "Lon", "H", "Q"])
        q = "%d" % int(d[11]) if d[11] > 0 else "NULL"
        t.add_row(["%0.3f" % d[1], "%.8f" % d[2], "%.8f" % d[3], "%.3f" % d[4], "%s" % q])
        return t.get_html_string()

    def from_array(self, data):
        col = data.shape[1]
        if col > 10:
            self.data = data
        else:
            self.data = np.zeros([data.shape[0], 11], np.double)
            self.data[:, :col] = data

    def from_nav(self, nav_path: str):
        data = np.genfromtxt(nav_path)
        self.from_array(data)

    def from_csv(self, csv_path: str):
        data = pd.read_csv(csv_path)
        self.from_array(data.values)

    def load_from_custom_txt(self, txt_path: str, column_index: list, skip_rows=0):
        data = np.genfromtxt(txt_path, skip_header=skip_rows)
        temp_data = np.zeros([data.shape[0], 20], np.double)
        for i, j in enumerate(column_index):
            temp_data[:, i] = data[:, j] if j > 0 else 0
        self.from_array(temp_data)


if __name__ == '__main__':
    # parse = NavParser()
    # parse.from_nav("/media/rebeater/hd_data2/workspace/raw_data/car/20201215/Couple/demo_1215/Loose/demo_1215.txt")
    # parse.split_by_time(206543.0,210021.832)
    # parse.to_nav("/media/rebeater/hd_data2/workspace/raw_data/car/20201215/Couple/demo_1215/Loose/demo_1215_03.nav")

    # data = np.loadtxt("/media/rebeater/hd_data2/workspace/raw_data/car/20201215/Rover/ICM_TD/20201215_3.data.gpstxt")
    # print(data.shape)
    # zeros = np.zeros([data.shape[0],1])
    # newdata = np.concatenate((zeros,data),axis=1)
    # print(newdata.shape)
    # with open("/media/rebeater/hd_data2/workspace/raw_data/car/20201215/Rover/ICM_TD/20201215_3.data.gpstxt.week.gpstxt",'w') as f:
    #     for d in newdata:
    #         f.write("%4d %5f %.12f %.12f %.8f %.4f %.4f %d %d\n"%(
    #                 d[0],d[1],d[2],d[3],d[4],d[5],d[6],d[7],d[8])
    #                 )

    data = np.loadtxt("/media/rebeater/hd_data2/workspace/raw_data/car/20201215/Rover/ICM_TD/20201215_3.data.prstxt")
    print(data.shape)
    zeros = np.zeros([data.shape[0], 1])
    # newdata = np.concatenate((zeros, data), axis=1)
    print(data.shape)
    with open(
            "/media/rebeater/hd_data2/workspace/raw_data/car/20201215/Rover/ICM_TD/20201215_3.data.prstxt.veltxt",
            'w') as f:
        for d in data:
            f.write("%5f %.5f %.5f\n" % (
               d[0],d[3],d[2])
                    )

    print(data.shape)
